www.gusucode.com > Robotics Playground工具箱matlab源码 > Robotics Playground/lib/Resources/Utils/addSensors.m
function addSensors(blk,robotID) % This funciton is meant to add a sensor visualization from the components % library to a robot % Copyright 2018 The MathWorks, Inc. % Disable the library link so all configurations can be saved with the % model, otherwise it pulls from the library when model opens set_param(blk,'LinkStatus','inactive'); % remove any existing visual blocks subsystem = [blk '/Sensor Visuals']; blocks = find_system(subsystem,'LookUnderMasks','all'); % Delete all previous blocks except the inport for idx = 2:length(blocks)-1 delete_block(blocks{idx}) end % Delete all signals while ~isempty(find_system(subsystem,'LookUnderMasks','all','FindAll','on','type','line')) lines = find_system(subsystem,'LookUnderMasks','all','FindAll','on','type','line'); delete_line(lines(1)) end % Set a position for the Physical connection port pos1 = [140 100 180 140]; pos2 = [140 170 180 210]; set_param(strcat(subsystem,'/Robot'),'position',[140 60 180 80]) %% Check for Distance sensors sensors=find_system(bdroot,'MaskType','RP Distance Sensor'); if ~isempty(sensors) % Check for block with a matching Robot ID for idx=1:size(sensors,1) if (eval(get_param(sensors{idx},'robotID')) == robotID) pos1 = pos1 + [60 0 60 0]; pos2 = pos2 + [60 0 60 0]; % Get X Y sensor offsets xyOffset=get_param(sensors{idx},'sensOff'); degOffset=get_param(sensors{idx},'sensOffDeg'); % Add sensor visual and set parameters add_block('RP_Simulator_Components/US Visual',strcat(subsystem,'/US Visual ',num2str(idx))); set_param(strcat(subsystem,'/US Visual ',num2str(idx)),'position',pos1,'Orientation','down'); set_param(strcat(subsystem,'/US Visual ',num2str(idx)),'pose',xyOffset,'angle',degOffset); add_line(subsystem,strcat('Robot','/Rconn1'),strcat('US Visual ',num2str(idx),'/LConn1')); end end end %% Check for contact switch sensors sensors=find_system(bdroot(gcs),'MaskType','RP Contact Switch'); if ~isempty(sensors) % Check for block with a matching Robot ID for idx=1:size(sensors,1) if (eval(get_param(sensors{idx},'robotID')) == robotID) pos1 = pos1 + [60 0 60 0]; pos2 = pos2 + [60 0 60 0]; % Get X Y sensor offsets xyOffset=get_param(sensors{idx},'sensOff'); degOffset=get_param(sensors{idx},'sensOffDeg'); % Add sensor visual and set parameters add_block('RP_Simulator_Components/Switch Visual',strcat(subsystem,'/Switch Visual ',num2str(idx))); set_param(strcat(subsystem,'/Switch Visual ',num2str(idx)),'position',pos1,'Orientation','down'); set_param(strcat(subsystem,'/Switch Visual ',num2str(idx)),'pose',xyOffset,'angle',degOffset); add_line(subsystem,strcat('Robot','/Rconn1'),strcat('Switch Visual ',num2str(idx),'/LConn1')); end end end %% Check for Compass sensors=find_system(bdroot(gcs),'MaskType','RP Compass Sensor'); if ~isempty(sensors) % Check for block with a matching Robot ID for idx=1:size(sensors,1) if (eval(get_param(sensors{idx},'robotID')) == robotID) pos1 = pos1 + [60 0 60 0]; pos2 = pos2 + [60 0 60 0]; % Add sensor visual and set parameters add_block('RP_Simulator_Components/Compass Visual',strcat(subsystem,'/Compass Visual ',num2str(idx))); set_param(strcat(subsystem,'/Compass Visual ',num2str(idx)),'position',pos1,'Orientation','down'); add_line(subsystem,strcat('Robot','/Rconn1'),strcat('Compass Visual ',num2str(idx),'/LConn1')); end end end %% Check for Object Sensor sensors=find_system(bdroot(gcs),'MaskType','RP Object Sensor'); if ~isempty(sensors) % Check for block with a matching Robot ID for idx=1:size(sensors,1) if (eval(get_param(sensors{idx},'robotID')) == robotID) pos1 = pos1 + [60 0 60 0]; pos2 = pos2 + [60 0 60 0]; % Get X Y sensor offsets xyOffset=get_param(sensors{idx},'sensOff'); degOffset=get_param(sensors{idx},'sensOffDeg'); % Add sensor visual and set parameters add_block('RP_Simulator_Components/Object Sensor Visual',strcat(subsystem,'/Object Sensor Visual ',num2str(idx))); set_param(strcat(subsystem,'/Object Sensor Visual ',num2str(idx)),'position',pos1,'Orientation','down'); set_param(strcat(subsystem,'/Object Sensor Visual ',num2str(idx)),'pose',xyOffset,'angle',degOffset); add_line(subsystem,strcat('Robot','/Rconn1'),strcat('Object Sensor Visual ',num2str(idx),'/LConn1')); end end end end